/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Just reads a MaCI pose and ranging files and stores it in "6D format" and in Sensor Frame
* 
* - very quick hack 
* 
*/
#include <math.h>
#include <vector>
#include "owndebug.h"
#include "ownutils.h"
#include "gimutils.h"
#include "CScanMeasurementSet.h"

void quaternionFromEuler(double r,double p,double y, double &q1, double &q2, double &q3, double &q4){
	q1 = cos(r/2.0)*cos(p/2.0)*cos(y/2.0) + sin(r/2.0)*sin(p/2.0)*sin(y/2.0);
	q2 = sin(r/2.0)*cos(p/2.0)*cos(y/2.0) - cos(r/2.0)*sin(p/2.0)*sin(y/2.0);
	q3 = cos(r/2.0)*sin(p/2.0)*cos(y/2.0) + sin(r/2.0)*cos(p/2.0)*sin(y/2.0);
	q4 = cos(r/2.0)*cos(p/2.0)*sin(y/2.0) - sin(r/2.0)*sin(p/2.0)*cos(y/2.0);
}

void eulerFromQuaternion(double &r,double &p,double &y, double q1, double q2, double q3, double q4){
	r = atan2(2.0*(q1*q2+q3*q4), 1.0-2.0*(q2*q2+q3*q3));
	p = asin(2.0*(q1*q3-q4*q2));
	y = atan2(2.0*(q1*q4+q2*q3),1.0-2.0*(q3*q3+q4*q4));
}

void NormalizeAngle(float &a){
	if(a > M_PI){
		while(a >  M_PI){
			a-= 2.0 * M_PI;
		}
	}
	else if(a < -M_PI){
		while(a < -M_PI){
			a += 2.0 * M_PI;
		}	
	}
}


int main(int argc, char *argv[]){
	/**
	* Position file only
	*/
	if(argc ==2){
		char *positionFile = argv[1];
		CPositionFileReader preader;
		preader.ReadFile(positionFile);
		
		std::list<gim::time> times;
		if(preader.GetTimes(times)){
			dPrint(1,"Got times... number of times %d",times.size());
		}else{
			dPrint(1,"Failed to get times ");
			exit(1);
		}
		
		MaCI::Position::TPose3D pData;
		std::list<gim::time>::iterator ii;
		
		for(ii=times.begin();ii!=times.end();ii++){
			preader.GetPositionData(pData,*ii);
			double q1,q2,q3,q4;
			NormalizeAngle(pData.eul_x);
			NormalizeAngle(pData.eul_y);
			NormalizeAngle(pData.eul_z);
			quaternionFromEuler(pData.eul_x,pData.eul_y,pData.eul_z,q1,q2,q3,q4);
			fprintf(stderr,"%u %u %f %f %f %f %f %f %f\n",(*ii).getSeconds(), (*ii).getUSeconds(), 
							pData.x, pData.y, pData.z,q1,q2,q3,q4 );
		}
	
		return 0;
	}
	
	if(argc !=3){
		fprintf(stderr,"\nUsage: ./poseConvert [in_position_file] [in_ranging_file]\n");
		exit(1);
	}
	char *positionFile = argv[1];
	char *rangingFile = argv[2];
	CScanMeasurementSet set(positionFile,rangingFile);
	
	Grid::pose p;
	double q1,q2,q3,q4;
	double r,pi,y;
	for(int i=0;i<set.size();i++){
		if(set.getPoseInLaserFrame(i, p)){
			p.toPI();
			quaternionFromEuler(0,0,p.a,q1,q2,q3,q4);
			//eulerFromQuaternion(r,pi,y,q1,q2,q3,q4);
			//fprintf(stderr,"A=%.2f (%.2f %.2f %.2f)\n",p.a,r,pi,y);
			fprintf(stderr,"%u %u %f %f %f %f %f %f %f\n",set.rTimes[i].getSeconds(), set.rTimes[i].getUSeconds(), 
							p.x, p.y, set.lz,q1,q2,q3,q4 );
			
			
		}else{
			dPrint(1,"No measurement");
		}
	}
	
	
return 0;
}